from microbit import *
import math

class PCA9685(object):
    def __init__(self, i2c, address, freq, init):
        self.address = address
        i2c.init(freq=100000, sda=pin20, scl=pin19)
        if not init:
            self.i2cW(0x00, 0x00)
            self.set_pwm_freq(freq)

    def i2cW(self, reg, value):
        buf = bytearray(2)
        buf[0] = reg
        buf[1] = value
        i2c.write(self.address, buf)

    def i2cR(self, reg):
        buf = bytearray(1)
        buf[0] = reg
        i2c.write(self.address, buf)
        return i2c.read(self.address, 1)

    def set_pwm_freq(self, freq):
        pre = math.floor(((25000000/4096/(freq * 0.915))-1) + 0.5)
        oldmode = self.i2cR(0x00)
        self.i2cW(0x00, (oldmode[0] & 0x7F) | 0x10)
        self.i2cW(0xFE, pre)
        self.i2cW(0x00, oldmode[0])
        sleep(5)
        self.i2cW(0x00, oldmode[0] | 0xa1)

    def set_pwm(self, channel, on, off):
        if ((channel < 0) or (channel > 15)):
            return
        buf = bytearray(5)
        buf[0] = 0x06 + 4 * channel
        buf[1] = on & 0xff
        buf[2] = (on >> 8) & 0xff
        buf[3] = off & 0xff
        buf[4] = (off >> 8) & 0xff
        i2c.write(self.address, buf)